package com.gitee.wsl.mathematics.geometry.d2.curve.ext

import com.gitee.wsl.mathematics.vector.vec3.Vec3f

fun derivative(x0: Vec3f, c0: Vec3f, x1: Vec3f, t: Double): Vec3f {
    val it = 1.0 - t
    return Vec3f(2 * it * (c0.x - x0.x) + 2 * t * (x1.x - c0.x),
        2 * it * (c0.y - x0.y) + 2 * t * (x1.y - c0.y),
        2 * it * (c0.z - x0.z) + 2 * t * (x1.z - c0.z)
    )
}

fun derivative(p0: Vec3f, p1: Vec3f, p2: Vec3f, p3: Vec3f, t: Double): Vec3f {
    val it = 1.0 - t
    return p1.minus(p0).times(3.0 * it * it).plus(p2.minus(p1).times(6.0 * it * t)).plus(p3.minus(p2).times(3.0 * t * t))
}

fun derivative2(p0: Vec3f, p1: Vec3f, p2: Vec3f, p3: Vec3f, t: Double): Vec3f {
    return (p2 - p1 * 2.0 + p0) * (6 * (1.0 - t)) + (p3 - p2 * 2.0 + p1) * (6.0 * t)
}

fun bezier(x0: Vec3f, c0: Vec3f, x1: Vec3f, t: Double): Vec3f {
    val it = 1.0 - t
    val it2 = it * it
    val t2 = t * t

    return Vec3f(
        it2 * x0.x + 2 * it * t * c0.x + t2 * x1.x,
        it2 * x0.y + 2 * it * t * c0.y + t2 * x1.y,
        it2 * x0.z + 2 * it * t * c0.z + t2 * x1.z)
}


/**
 * Samples a single point based on the provided
 * [t](https://pomax.github.io/bezierinfo/#explanation) value
 * from given 3D cubic Bézier curve.
 *
 * @param x0 The starting anchor point of the curve.
 * @param c0 The first control point.
 * @param c1 The second control point.
 * @param x1 The ending anchor point of the curve.
 * @param t The value of *t* in the range of `0.0` to `1.0`.
 * @return A sample on the curve.
 */
fun bezier(x0: Vec3f, c0: Vec3f, c1: Vec3f, x1: Vec3f, t: Double): Vec3f {
    val it = 1.0 - t
    val it2 = it * it
    val it3 = it2 * it
    val t2 = t * t
    val t3 = t2 * t

    return Vec3f(
        it3 * x0.x + 3 * it2 * t * c0.x + 3 * it * t2 * c1.x + t3 * x1.x,
        it3 * x0.y + 3 * it2 * t * c0.y + 3 * it * t2 * c1.y + t3 * x1.y,
        it3 * x0.z + 3 * it2 * t * c0.z + 3 * it * t2 * c1.z + t3 * x1.z)
}